use hikrgbd::*;
use hikrgbd::mv3drgbd::Mv3dRgbdImageType;
use std::thread;
use std::time::Duration;
use image;

fn main() {
    // 创建相机实例
    let mut camera = RGBDCamera::new().expect("创建相机失败");
    // 打开相机
    camera.open().expect("打开相机失败");
    
    // 开始采集
    camera.start().expect("启动采集失败");
    
    // 主动获取帧3图像并保存
    for i in 0..3 {
        match camera.fetch_frame(5000) {
            Ok(frame) => {
                println!("\n获取第{}帧图像成功:", i + 1);
                // 显示图像信息
                for j in 0..frame.nImageCount {
                    let image = &frame.stImageData[j as usize];
                    println!("  [{}] 图像信息: 帧号: {}, 类型: {:?}, 分辨率: {}x{}, 数据长度: {} 字节, 流类型: {:?}",
                                j,
                                image.nFrameNum,
                                image.enImageType,
                                image.nWidth,
                                image.nHeight,
                                image.nDataLen,
                                image.enStreamType);

                    // 保存图像到根目录下
                    // 获取图像数据切片指针
                    let data = unsafe { std::slice::from_raw_parts(image.pData, image.nDataLen as usize) };
                    // 根据图像类型进行处理
                    if image.enImageType == Mv3dRgbdImageType::ImageType_YUV422 {
                        println!("  处理彩色图像...");
                        let rgb_data = convert_yuv422_to_rgb(data, image.nWidth, image.nHeight);
                        println!("  转换后的RGB数据大小: {}", rgb_data.len());

                        // 保存图像到根目录下
                        let filename = format!("image_rgb_{}.jpg", i);
                        let img = image::RgbImage::from_raw(
                            image.nWidth as u32,
                            image.nHeight as u32,
                            rgb_data
                        ).expect("创建图像失败");
                        img.save(&filename).expect("保存图像失败");
                    }

                    if image.enImageType == Mv3dRgbdImageType::ImageType_Depth {
                        println!("  处理深度图像...");
                        let rgb_data = convert_depth_to_rgb(data, image.nWidth, image.nHeight);
                        println!("  转换后的RGB数据大小: {}", rgb_data.len());

                        // 转换成点云图像
                         match camera.depth_to_pointcloud(image) {
                            Ok(pointcloud) => {
                                match camera.get_point_xyz(&pointcloud, 763, 378) {
                                    Ok((x, y, z)) => {
                                        println!("\x1b[36m[点云坐标] 像素点(763, 378) -> X: {:.2}mm, Y: {:.2}mm, Z: {:.2}mm\x1b[0m",
                                            x, y, z);
                                    }
                                    Err(e) => println!("获取点云坐标失败: {}", e),
                                }
                            }
                            Err(e) => println!("转换点云数据失败: {}", e),
                         };
                        
                        let filename = format!("image_depth_{}.jpg", i);
                        let img = image::RgbImage::from_raw(
                            image.nWidth as u32,
                            image.nHeight as u32,
                            rgb_data
                        ).expect("创建图像失败");
                        img.save(&filename).expect("保存图像失败");
                    }
                }
            }
            Err(e) => println!("获取图像失败: {}", e),
        }
        thread::sleep(Duration::from_millis(100));
    }
    
    // 停止采集
    camera.stop().expect("停止采集失败");
    // 关闭相机
    camera.close().expect("关闭相机失败");
}